﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathLib;

namespace TrackingSim.Filtering.Kinematics
{
    class State_R6 : KinematicState
    {
        public State_R6()
            : base(0.0, 6)
        {

        }

        public State_R6(double _time)
            : base(_time, 6)
        {

        }

        public override KinematicState clone()
        {
            State_R6 copy = new State_R6(Time);

            copy.State = State;
            copy.Cov = Cov;

            return copy;
        }

        public Vector Pos
        {
            get
            {
                Vector pos = new Vector(3);
                pos.v[0] = X;
                pos.v[1] = Y;
                pos.v[2] = Z;
                return pos;
            }

            set
            {
                if (value.N == 3)
                {
                    X = value.v[0];
                    Y = value.v[1];
                    Z = value.v[2];
                }
                else
                {
                    throw new IndexOutOfRangeException("Vector lengths do not agree.");
                }
            }
        }

        public Vector Vel
        {
            get
            {
                Vector vel = new Vector(3);
                vel.v[0] = Xdot;
                vel.v[1] = Ydot;
                vel.v[2] = Zdot;
                return vel;
            }

            set
            {
                if (value.N == 3)
                {
                    Xdot = value.v[0];
                    Ydot = value.v[1];
                    Zdot = value.v[2];
                }
                else
                {
                    throw new IndexOutOfRangeException("Vector lengths do not agree.");
                }
            }
        }

        public double X
        {
            get { return State.X; }
            set { State.X = value; }
        }

        public double Y
        {
            get { return State.Y; }
            set { State.Y = value; }
        }

        public double Z
        {
            get { return State.Z; }
            set { State.Z = value; }
        }

        public double Xdot
        {
            get { return State.v[3]; }
            set { State.v[3] = value; }
        }

        public double Ydot
        {
            get { return State.v[4]; }
            set { State.v[4] = value; }
        }

        public double Zdot
        {
            get { return State.v[5]; }
            set { State.v[5] = value; }
        }
    }
}
